﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Webcam.Image.Filters
{
    /// <summary>An edge tracker</summary>
    public class EdgeTracker : ImageAction
    {
        /// <summary>Create an edge tracker object</summary>
        /// <param name="src"></param>
        public EdgeTracker(RGBImage src)
            : base(src)
        {
        }

        /// <summary>The lines found</summary>
        private List<Line> lines = new List<Line>();

        /// <summary>The lines found</summary>
        public Line[] Lines
        { get { return lines.ToArray(); } }

        /// <summary>Process the image for edges</summary>
        public override void Process()
        {
            FindLines();
            ReduceDuplicates();
        }

        public void AddLines()
        {
            lines.Add(new Line(new Point(40, 40), new Point(80, 80)));
            lines.Add(new Line(new Point(50, 50), new Point(100, 100)));
            lines.Add(new Line(new Point(60, 60), new Point(120, 115)));
            lines.Add(new Line(new Point(70, 70), new Point(70, 100)));
        }

        /// <summary>
        /// Set the collection of lines
        /// </summary>
        /// <param name="lines"></param>
        public void SetLines(Line[] lines)
        {
            this.lines.Clear();
            foreach (Line l in lines)
            {
                this.lines.Add(l);
            }
        }

        /// <summary>
        /// Remove duplicate lines
        /// </summary>
        public void ReduceDuplicates()
        {
            List<LineCache> tempLines = new List<LineCache>();
            foreach (Line l in lines)
            {
                LineCache lc = new LineCache(l);
                lc.CalcLine();
                tempLines.Add(lc);
            }

            List<LineCache> output = new List<LineCache>();

            // all lines are now sorted according to gradients
            var sortedGradients = from c in tempLines
                                  orderby c.angleFromHorizontal descending
                                  select c;

            foreach (LineCache ll in sortedGradients)
            {
                bool similar = false;
                LineCache lc = ll;

                foreach (LineCache current in output)
                {
                    if (LinesSimilar(lc, current))
                    {
                        similar = true;
                        // then same line area
                        // extend using maximum and minimum x values
                        Point[] points = new Point[4];
                        points[0] = lc.line.p1;
                        points[1] = lc.line.p2;
                        points[2] = current.line.p1;
                        points[3] = current.line.p2;

                        Point minPoint = points[0];
                        Point maxPoint = points[0];
                        for (int i = 1; i < 4; i++)
                        {
                            Point p = points[i];

                            if (minPoint.X > p.X)
                                minPoint = p;

                            if (maxPoint.X < p.X)
                                maxPoint = p;
                        }

                        // so these points are the new values for this line
                        // so update the line coordinates
                        current.line.p1 = minPoint;
                        current.line.p2 = maxPoint;
                        current.CalcLine();

                    }
                }
                if (!similar)
                {
                    output.Add(ll);
                }

            }

            lines.Clear();
            foreach (LineCache lc in output)
            {
                lines.Add(lc.line);
            }
        }

        /// <summary>The angle difference allowed before 2 lines are considered in the same direction</summary>
        float angleSim = 10.0f * 0.0174532925f;

        /// <summary>The distance between 2 lines before they are considered together</summary>
        float distanceBetweenLines = 3;

        /// <summary>Are these 2 lines similar?</summary>
        /// <param name="l1"></param>
        /// <param name="l2"></param>
        /// <returns></returns>
        bool LinesSimilar(LineCache l1, LineCache l2)
        {
            float a1 = l1.angleFromHorizontal;
            float a2 = l2.angleFromHorizontal;
            if (a1 < 0)
                a1 += (float)Math.PI;
            if (a2 < 0)
                a2 += (float)Math.PI;
            if (Maths.FloatSimilar(a1, a2, angleSim))
            {
                // similar gradients
                bool near = false;
                int xDiff = l1.Diff.X;
                int yDiff = l1.Diff.Y;
                if (xDiff < 0)
                    xDiff = -xDiff;
                if (yDiff < 0)
                    yDiff = -yDiff;
                if (xDiff > yDiff && !l1.isVertical)
                {
                    // then flat line
                    // check y coordinates
                    int xStart;
                    int xEnd;
                    if (l1.line.p1.X < l2.line.p2.X)
                    {
                        xStart = l1.line.p1.X;
                        xEnd = l1.line.p2.X;
                    }
                    else
                    {
                        xStart = l1.line.p2.X;
                        xEnd = l1.line.p1.X;
                    }

                    int xMiddle = (xStart + xEnd) / 2;
                    int[] xpos = new int[3];
                    xpos[0] = xStart;
                    xpos[1] = xMiddle;
                    xpos[2] = xEnd;
                    foreach (int x in xpos)
                    {
                        int yL1 = Maths.CalculateLineYPoint(x, l1.Gradient, l1.c);
                        int yL2 = Maths.CalculateLineYPoint(x, l2.Gradient, l2.c);

                        int diff = yL1 - yL2;
                        if (diff < 0)
                            diff = -diff;

                        if (diff < distanceBetweenLines)
                        {
                            near = true;
                            break;
                        }
                    }
                }
                else
                {
                    // then more vertical line
                    int yStart;
                    int yEnd;
                    if (l1.line.p1.Y < l2.line.p2.Y)
                    {
                        yStart = l1.line.p1.Y;
                        yEnd = l1.line.p2.Y;
                    }
                    else
                    {
                        yStart = l1.line.p2.Y;
                        yEnd = l1.line.p1.Y;
                    }

                    int yMiddle = (yStart + yEnd) / 2;
                    int[] ypos = new int[3];
                    ypos[0] = yStart;
                    ypos[1] = yMiddle;
                    ypos[2] = yEnd;
                    foreach (int y in ypos)
                    {
                        int xL1 = Maths.CalculateLineXPoint(y, l1.Gradient, l1.c);
                        int xL2 = Maths.CalculateLineXPoint(y, l2.Gradient, l2.c);

                        int diff = xL1 - xL2;

                        if (diff < 0)
                            diff = -diff;

                        if (diff < distanceBetweenLines)
                        {
                            near = true;
                            break;
                        }
                    }
                }


                if (near)
                {
                    // similar c
                    // so now, if the x's overlap
                    // then is the same line
                    if (VectorMath.OverlappingPairs(l2.line.p1.X, l2.line.p2.X, l1.line.p1.X, l1.line.p2.X))
                    {
                        if (VectorMath.OverlappingPairs(l2.line.p1.Y, l2.line.p2.Y, l1.line.p1.Y, l1.line.p2.Y))
                        {
                            return true;
                        }
                    }
                }
            }
            return false;
        }

        /// <summary>
        /// Cache's some line data
        /// </summary>
        private class LineCache
        {
            public LineCache(Line l)
            {
                this.line = l;
            }
            public float angleFromHorizontal;
            public Line line = null;
            public float Gradient = 0.0f;
            public bool isVertical = false;

            public Point Diff = Point.Empty;

            public float c = 0.0f;
            public void CalcLine()
            {
                Diff = Point.Subtract(line.p2, line.p1);
                if (Diff.X == 0)
                {
                    isVertical = true;
                    this.Gradient = 1000000000;
                    c = line.p1.X;
                    angleFromHorizontal = 90.0f;
                }
                else
                {
                    this.Gradient = Maths.CalculateLineGradient(line.p1, line.p2);
                    this.c = Maths.CalculateLineC(line.p1, this.Gradient);
                    this.angleFromHorizontal = VectorMath.aTan(Gradient);
                }
            }

        }


        /// <summary>Find the maximum point in the box</summary>
        /// <param name="x">Left Coordinate</param>
        /// <param name="y">Top Coordinate</param>
        /// <param name="boxWidth">The width of the box</param>
        /// <returns></returns>
        private Point MaxPoint(int _x, int _y, int boxWidth)
        {
            int xMax = _x + boxWidth;
            int yMax = _y + boxWidth;

            Point max = new Point(_x, _y);
            byte maxVal = 0;
            for (int x = _x; x < xMax; x++)
            {
                for (int y = _y; y < yMax; y++)
                {
                    if (src[x, y].GrayScale > maxVal)
                    {
                        // then this is a new maximum
                        maxVal = src[x, y].GrayScale;
                        max = new Point(x, y);
                        if (maxVal == byte.MaxValue)
                        {
                            // the max value posible for a byte
                            // so cannot get any larger
                            // so return this point
                            return max;
                        }
                    }
                }
            }
            return max;
        }

        /// <summary>The intensity requirement for a starting point of a line</summary>
        public byte StartingPointIntensityMinimum = 150;

        /// <summary>The range from one line to another for it to be 'near'</summary>
        public int NearLineRange = 5;

        /// <summary>Is the point near line that has already been found</summary>
        /// <param name="p"></param>
        /// <returns></returns>
        bool IsNearLine(Point p)
        {
            foreach (Line l in lines)
            {
                if (VectorMath.DistancePointToLine(l, p) < NearLineRange)
                {
                    if (p.X > l.p1.X - NearLineRange && p.X < l.p2.X + NearLineRange)
                    {
                        if (p.Y > l.p1.Y - NearLineRange && p.Y < l.p2.Y + NearLineRange)
                            return true;
                    }
                }
            }
            return false;
        }

        /// <summary>The size of the boxes</summary>
        public int GridSize = 40;

        /// <summary>Find the lines</summary>
        private void FindLines()
        {
            int xRes = src.Width / GridSize;
            int yRes = src.Height / GridSize;

            Point[,] maxPoints = new Point[xRes, yRes];
            List<Point> points = new List<Point>();
            for (int x = 0; x < xRes; x++)
            {
                for (int y = 0; y < yRes; y++)
                {
                    int _x = x * GridSize;
                    int _y = y * GridSize;
                    Point p = MaxPoint(_x, _y, GridSize);
                    maxPoints[x, y] = p;

                    points.Add(p);
                }
            }

            var stuff = from item in points
                        orderby src[item].GrayScale descending
                        select item;



            PosiblePointsChecked.Clear();
            foreach (Point p in stuff)
            {
                if (src[p].GrayScale < 50)
                    continue;
                if (IsNearLine(p))
                    continue;
                PosiblePointsChecked.Add(p);
                FindLine(p);
            }
        }

        List<Point> PosiblePointsChecked = new List<Point>();

        /// <summary>Minimum Line Intensity for any point along a line</summary>
        byte LineIntensity_Minimum = 0;

        /// <summary>The width of the initial check</summary>
        public int StartingRadius = 30;

        private void FindLine(Point StartingPoint)
        {
            List<Point> points = new List<Point>();
            points.Add(StartingPoint);

            List<Point> posPoints = new List<Point>();

            // find the left,right,top and bottom of the initial box
            int left = StartingPoint.X - StartingRadius;
            int right = StartingPoint.X + StartingRadius;
            int top = StartingPoint.Y - StartingRadius;
            int bottom = StartingPoint.Y + StartingRadius;

            left = Maths.Trim(left, 0, src.Width - 1);
            right = Maths.Trim(right, 0, src.Width - 1);
            top = Maths.Trim(top, 0, src.Height - 1);
            bottom = Maths.Trim(bottom, 0, src.Height - 1);

            // add the points along the top and bottom sides
            for (int i = left; i <= right; i++)
            {
                posPoints.Add(new Point(i, top));
                posPoints.Add(new Point(i, bottom));
            }
            // add the points along the left and right sides
            for (int i = top + 1; i <= bottom - 1; i++)
            {
                posPoints.Add(new Point(left, i));
                posPoints.Add(new Point(right, i));
            }

            // now remove any points around the starting point that aren't
            // of suitable intensity
            List<Point> validPoints = new List<Point>();
            foreach (Point p in posPoints)
            {
                if (src[p].GrayScale > LineIntensity_Minimum)
                {
                    validPoints.Add(p);
                }
            }

            List<PointChance> LinePoints = new List<PointChance>();
            // now check the remaining points for points that reach back to the
            // starting square. Making all posiblities in this list valid starting points
            foreach (Point p in validPoints)
            {
                int t = CheckLineToPoint(StartingPoint, p);
                LinePoints.Add(new PointChance(p, t));
            }

            var SortedLinePoints = from s in LinePoints
                                   where s.Chance > 0
                                   orderby s.Chance descending
                                   select s;

            PointChance pointFound = null;
            foreach (PointChance pc in SortedLinePoints)
            {
                pointFound = pc;
                //lines.Add(new Line(StartingPoint, pc.point, RGB.Red));
                break;
            }
            if (pointFound == null)
                return;

            int NewChance;
            int newNewChance;
            Line l = FollowLine(new Line(StartingPoint, pointFound.point), pointFound.Chance, out NewChance);
            Line l2 = FollowLine(new Line(l.p2, l.p1), NewChance, out newNewChance);
            lines.Add(l2);
        }
        /// <summary>
        /// The distance traveled in every jump when following a line
        /// </summary>
        int Jump = 3;
        /// <summary>
        /// The sideways range checked when following a line
        /// </summary>
        int JumpReach = 2;
        /// <summary>
        /// Follow a line
        /// </summary>
        /// <param name="line"></param>
        /// <returns></returns>
        public Line FollowLine(Line line, int InitialStrength, out int Chance)
        {
            Point locked = line.p1;
            Point current = line.p2;

            List<Point> posiblePoints = new List<Point>(JumpReach * 2 + 1);

            Point diff = Point.Subtract(current, locked);


            if (diff.X == 0)
            {
                // then infinitive gradient
                // so vertical line
                Point change;
                if (diff.Y < 0) // moving down
                    change = new Point(-Jump, 0);
                else // moving up
                    change = new Point(Jump, 0);
                Point nxtPoint = Point.Add(change, current);

                // vertical line
                // so add lines either side of the nxt point
                posiblePoints.Add(nxtPoint);
                for (int i = 1; i <= JumpReach; i++)
                {
                    posiblePoints.Add(Point.Subtract(nxtPoint, new Point(i, 0)));
                    posiblePoints.Add(Point.Add(nxtPoint, new Point(i, 0)));
                }

            }
            else
            {
                float m = Maths.CalculateLineGradient(diff);
                float c = Maths.CalculateLineC(locked, m);

                if (diff.X >= diff.Y)
                {
                    // then the line is more x than y
                    // so mov along the x axis
                    int x = current.X;

                    if (diff.X < 0)
                        x -= Jump;
                    else
                        x += Jump;

                    Point nxtPoint = new Point(x, Maths.CalculateLineYPoint(x, m, c));

                    // since moving more x than y
                    // then flatish gradient
                    // so add points above and below this point
                    for (int i = 1; i <= JumpReach; i++)
                    {
                        posiblePoints.Add(Point.Subtract(nxtPoint, new Point(0, i)));
                        posiblePoints.Add(Point.Add(nxtPoint, new Point(0, i)));
                    }
                }
                else
                {
                    // then the line is more y than x
                    // so move along the y axis
                    int y = current.Y;

                    if (diff.Y < 0)
                        y -= Jump;
                    else
                        y += Jump;

                    Point nxtPoint = new Point(Maths.CalculateLineXPoint(y, m, c), y);
                    // since moving more y than x
                    // then largish gradient
                    // so add points left and right of this point
                    posiblePoints.Add(nxtPoint);
                    for (int i = 1; i <= JumpReach; i++)
                    {
                        posiblePoints.Add(Point.Subtract(nxtPoint, new Point(i, 0)));
                        posiblePoints.Add(Point.Add(nxtPoint, new Point(i, 0)));
                    }
                }
            }

            List<PointChance> pointScores = new List<PointChance>();
            foreach (Point p in posiblePoints)
            {
                if (p.X < 0 || p.X >= src.Width || p.Y < 0 || p.Y >= src.Height)
                {
                    // line has left the bounds of the image
                    // so not a posible point
                    continue;
                }
                PointChance pc = new PointChance(p, this.CheckLineToPoint(locked, p));
                pointScores.Add(pc);
            }

            var sortedScores = from o in pointScores
                               where o.Chance > this.LineIntensity_Minimum
                               orderby o.Chance descending
                               select o;

            PointChance bestPoint = null;
            foreach (PointChance s in sortedScores)
            {
                bestPoint = s;
                break;
            }
            if (bestPoint == null)
            {
                // no better point in the area
                // return the previous position
                Chance = InitialStrength;
                return line;
            }


            if (bestPoint.Chance > InitialStrength)
            {
                // the next point along the line is still valid
                // so check the new lines progress
                int newChance;
                Line l = FollowLine(new Line(locked, bestPoint.point), bestPoint.Chance, out newChance);
                Chance = newChance;
                return l;
            }
            else
            {
                // then the line has ended
                Chance = InitialStrength;
                return line;
            }

        }

        /// <summary>Contains a point and a chance rating</summary>
        class PointChance
        {
            public Point point;
            /// <summary>Greater chance means point is more likely to be here</summary>
            public int Chance;
            public PointChance(Point p, int chance)
            {
                this.point = p;
                this.Chance = chance;
            }
        }

        /// <summary>Check there is evidence of a straightish line from p1 to p2</summary>
        /// <param name="p1"></param>
        /// <param name="p2"></param>
        /// <returns></returns>
        int CheckLineToPoint(Point p1, Point p2)
        {
            Point[] points = Maths.PointsAlongLine(p1, p2);

            int LineyLikelyness = 0;
            int BlankPxs = 0;
            int MaxBlankPxs = (int)(StartingRadius * PercentageAllowedToBeBlank);
            foreach (Point p in points)
            {
                byte v = src[p].GrayScale;
                if (v > LineIntensity_Minimum)
                    LineyLikelyness += v;
                else
                {
                    BlankPxs++;
                    if (BlankPxs > MaxBlankPxs)
                        return 0;
                }
            }
            return LineyLikelyness;
        }

        /// <summary>The percentage of px's allowed to be blank</summary>
        /// <value>0.0-1.0</value>
        public float PercentageAllowedToBeBlank = 0.05f;

        /// <summary>
        /// Represent the findings of this function as an image
        /// </summary>
        /// <returns></returns>
        public RGBImage ToRGBImage()
        {
            RGBImage img = this.src.Clone();
            
            foreach (Line l in lines)
            {
                //GraphicsDrawing.DrawPoint(img, 5, l.p1, RGB.Green);
                //GraphicsDrawing.DrawPoint(img, 5, l.p2, RGB.Green);
                GraphicsDrawing.DrawLine(img, l);
            }

            return img;
        }

        /// <summary>
        /// Convert the edge tracker to a graphical representation of the lines
        /// </summary>
        public override RGBImage Output
        {
            get
            {
                return ToRGBImage();
            }
            protected set
            {
                base.Output = value;
            }
        }
    }
}
